// 2025/3/25: 首个记录
// 2025/4/19: 新增大灯位置坐标和相对于地面俯仰角字段
// 2025/6/12: 协议名常量改为public

package com.appox.samples;

import com.appox.Sample;
import com.appox.Types.FloatPoint;
import com.appox.GeneralSample;
import com.appox.GeneralElement;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

/**
 * 车辆样本数据类
 */
public class VehicleSample extends Sample {

    /**
     * 档位
     */
    public enum GearPosition {
        UNKNOWN(0), // 未知
        PARK(1), // P档
        REVERSE(2), // R档
        NEUTRAL(3), // N档
        DRIVE(4); // D档
        
        private final int value;
        
        GearPosition(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static GearPosition fromValue(int value) {
            for (GearPosition gear : GearPosition.values()) {
                if (gear.getValue() == value) {
                    return gear;
                }
            }
            return UNKNOWN;
        }
    }
    
    /**
     * 转向灯状态
     */
    public enum TurnState {
        UNKNOWN(0), // 未知
        NONE(1), // 未打灯
        TURN_LEFT(2), // 左转向灯
        TURN_RIGHT(3), // 右转向灯
        EMERGENCY(4); // 双闪
        
        private final int value;
        
        TurnState(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static TurnState fromValue(int value) {
            for (TurnState turn : TurnState.values()) {
                if (turn.getValue() == value) {
                    return turn;
                }
            }
            return UNKNOWN;
        }
    }
    
    /**
     * 喇叭状态
     */
    public enum Horn {
        UNKNOWN(0), // 未知
        NONE(1), // 不响
        ACTIVE(2); // 响
        
        private final int value;
        
        Horn(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static Horn fromValue(int value) {
            for (Horn horn : Horn.values()) {
                if (horn.getValue() == value) {
                    return horn;
                }
            }
            return UNKNOWN;
        }
    }
    
    /**
     * 大灯状态
     */
    public enum HeadLight {
        UNKNOWN(0), // 未知
        NONE(1), // 未激活
        NEAR(2), // 近光
        FAR(3); // 远光
        
        private final int value;
        
        HeadLight(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static HeadLight fromValue(int value) {
            for (HeadLight light : HeadLight.values()) {
                if (light.getValue() == value) {
                    return light;
                }
            }
            return UNKNOWN;
        }
    }
    
    /**
     * 雨刮状态
     */
    public enum Wiper {
        UNKNOWN(0), // 未知
        NONE(1), // 未激活
        SINGLE(2), // 单次
        SLOW1(3), // 慢 1
        SLOW2(4), // 慢 2
        SLOW3(5), // 慢 3
        SLOW4(6), // 慢 4
        SLOW5(7), // 慢 5
        MEDIUM(8), // 中档
        FAST(9); // 快档
        
        private final int value;
        
        Wiper(int value) {
            this.value = value;
        }
        
        public int getValue() {
            return value;
        }
        
        public static Wiper fromValue(int value) {
            for (Wiper wiper : Wiper.values()) {
                if (wiper.getValue() == value) {
                    return wiper;
                }
            }
            return UNKNOWN;
        }
    }
    
    // 基本属性
    private double speed; // [kph] 本车车速
    private double kilometerAge; // [km] 总里程数
    private Double yawRate; // [deg/s] 横摆角速度
    private Double curvature; // [1/m] 行驶曲率，左转为正
    private Double maxCurvature; // [1/m] 最大行驶曲率，不分左右（常量）
    private Double accelX; // [m/s2] 本车纵向加速度
    private Double maxAcceleration; // [m/s2] 前向行驶最大加速度（常量）
    private Double maxDeceleration; // [m/s2] 前向行驶最大减速度（常量）
    private Double accelY; // [m/s2] 本车横向加速度
    private Double maxAccelY; // [m/s2] 最大允许横向加速度，不分左右（常量）
    private Double jerkX; // [m/s3] 本车纵向急动度
    private Double jerkY; // [m/s3] 本车横向急动度
    private FloatPoint[] trajectory; // 历史路径
    private FloatPoint[] contour; // 轮廓描述

    // 车辆尺寸参数
    private double vehicleWidth; // [m] 车宽（常量）
    private double vehicleLength; // [m] 车长（常量）
    private double vehicleHeight; // [m] 车高（常量）
    private double steerAngleRatio; // 传动比（常量）
    private double wheelBase; // [m] 轴距（常量）
    private double rearTread; // [m] 轮距（常量）
    private double frontOverhang; // [m] 前悬（常量）

    // 方向盘参数
    private Double steerAngle; // [deg] 方向盘转角
    private Double maxSteerAngle; // [deg] 最大方向盘转角（常量）
    private Double steerAngleRate; // [deg/s] 方向盘转角速度
    private Double maxSteerAngleRate; // [deg/s] 最大方向盘转角速度（常量）
    private Double steerTorque; // [Nm] 方向盘力矩

    // 车辆控制参数
    private Double brake; // [%] 刹车位置
    private Double brakeDeadZone; // [%] 刹车踏板死区（常量）
    private Double throttle; // [%] 油门位置
    private Double throttleDeadZone; // [%] 油门踏板死区（常量）
    private GearPosition gear; // 档位
    private TurnState turn; // 转向灯
    private Horn horn; // 喇叭
    private HeadLight headLight; // 大灯
    private Wiper wiper; // 雨刮

    // 车轮参数
    private Double flSpeed; // [kph] 左前轮轮速
    private Double frSpeed; // [kph] 右前轮轮速
    private Double rlSpeed; // [kph] 左后轮轮速
    private Double rrSpeed; // [kph] 右后轮轮速

    private Double flMass; // [kg] 左前轮上的质量（常量）
    private Double frMass; // [kg] 右前轮上的质量（常量）
    private Double rlMass; // [kg] 左后轮上的质量（常量）
    private Double rrMass; // [kg] 右后轮上的质量（常量）

    private Double flAngle; // [deg] 左前轮朝向角
    private Double frAngle; // [deg] 右前轮朝向角

    // 发动机参数
    private Double engineSpeed; // [rpm] 引擎转速
    private Double engineTorque; // [Nm] 引擎扭矩

    // 其他参数
    private Double frontEndLateralSpeed; // [m/s] 前保中心横向速度
    private Double rearEndLateralSpeed; // [m/s] 后保中心横向速度

    private Double frontCorneringStiffness; // [N/rad] 前轮侧偏刚度，左右轮之和（常量）
    private Double rearCorneringStiffness; // [N/rad] 后轮侧偏刚度，左右轮之和（常量）

    private Double flLightPosX; // [m] 左前灯X坐标(即右前灯X坐标)
    private Double flLightPosY; // [m] 左前灯Y坐标(右前灯Y坐标为其负值)
    private Double flLightPosZ; // [m] 左前灯Z坐标(即右前灯Z坐标)
    private Double rlLightPosX; // [m] 左后灯X坐标(即右后灯X坐标)
    private Double rlLightPosY; // [m] 左后灯Y坐标(右后灯Y坐标为其负值)
    private Double rlLightPosZ; // [m] 左后灯Z坐标(即右后灯Z坐标)

    private Double pitchToGround; // [deg] 车辆相对于地面的俯仰角

    /**
     * 构造函数，初始化默认值
     */
    public VehicleSample() {
        // 设置默认值
        this.vehicleWidth = 1.9;
        this.vehicleLength = 4.6;
        this.vehicleHeight = 1.5;
        this.steerAngleRatio = 15;
        this.wheelBase = 2.8;
        this.rearTread = 1.6;
        this.frontOverhang = 0.9;
        this.gear = GearPosition.UNKNOWN;
        this.turn = TurnState.UNKNOWN;
        this.horn = Horn.UNKNOWN;
        this.headLight = HeadLight.UNKNOWN;
        this.wiper = Wiper.UNKNOWN;
        this.contour = new FloatPoint[0];
        this.trajectory = new FloatPoint[0];
    }
    
    // Getter和Setter方法
    public double getSpeed() {
        return speed;
    }

    public void setSpeed(double speed) {
        this.speed = speed;
    }

    public double getKilometerAge() {
        return kilometerAge;
    }

    public void setKilometerAge(double kilometerAge) {
        this.kilometerAge = kilometerAge;
    }

    public Double getYawRate() {
        return yawRate;
    }

    public void setYawRate(Double yawRate) {
        this.yawRate = yawRate;
    }

    public Double getCurvature() {
        return curvature;
    }

    public void setCurvature(Double curvature) {
        this.curvature = curvature;
    }

    public Double getMaxCurvature() {
        return maxCurvature;
    }

    public void setMaxCurvature(Double maxCurvature) {
        this.maxCurvature = maxCurvature;
    }

    public Double getAccelX() {
        return accelX;
    }

    public void setAccelX(Double accelX) {
        this.accelX = accelX;
    }

    public Double getMaxAcceleration() {
        return maxAcceleration;
    }

    public void setMaxAcceleration(Double maxAcceleration) {
        this.maxAcceleration = maxAcceleration;
    }

    public Double getMaxDeceleration() {
        return maxDeceleration;
    }

    public void setMaxDeceleration(Double maxDeceleration) {
        this.maxDeceleration = maxDeceleration;
    }

    public Double getAccelY() {
        return accelY;
    }

    public void setAccelY(Double accelY) {
        this.accelY = accelY;
    }

    public Double getMaxAccelY() {
        return maxAccelY;
    }

    public void setMaxAccelY(Double maxAccelY) {
        this.maxAccelY = maxAccelY;
    }

    public Double getJerkX() {
        return jerkX;
    }

    public void setJerkX(Double jerkX) {
        this.jerkX = jerkX;
    }

    public Double getJerkY() {
        return jerkY;
    }

    public void setJerkY(Double jerkY) {
        this.jerkY = jerkY;
    }

    public FloatPoint[] getTrajectory() {
        return trajectory;
    }

    public void setTrajectory(FloatPoint[] trajectory) {
        this.trajectory = trajectory;
    }

    public FloatPoint[] getContour() {
        return contour;
    }

    public void setContour(FloatPoint[] contour) {
        this.contour = contour;
    }

    public double getVehicleWidth() {
        return vehicleWidth;
    }

    public void setVehicleWidth(double vehicleWidth) {
        this.vehicleWidth = vehicleWidth;
    }

    public double getVehicleLength() {
        return vehicleLength;
    }

    public void setVehicleLength(double vehicleLength) {
        this.vehicleLength = vehicleLength;
    }

    public double getVehicleHeight() {
        return vehicleHeight;
    }

    public void setVehicleHeight(double vehicleHeight) {
        this.vehicleHeight = vehicleHeight;
    }

    public double getSteerAngleRatio() {
        return steerAngleRatio;
    }

    public void setSteerAngleRatio(double steerAngleRatio) {
        this.steerAngleRatio = steerAngleRatio;
    }

    public double getWheelBase() {
        return wheelBase;
    }

    public void setWheelBase(double wheelBase) {
        this.wheelBase = wheelBase;
    }

    public double getRearTread() {
        return rearTread;
    }

    public void setRearTread(double rearTread) {
        this.rearTread = rearTread;
    }

    public double getFrontOverhang() {
        return frontOverhang;
    }

    public void setFrontOverhang(double frontOverhang) {
        this.frontOverhang = frontOverhang;
    }

    public Double getSteerAngle() {
        return steerAngle;
    }

    public void setSteerAngle(Double steerAngle) {
        this.steerAngle = steerAngle;
    }

    public Double getMaxSteerAngle() {
        return maxSteerAngle;
    }

    public void setMaxSteerAngle(Double maxSteerAngle) {
        this.maxSteerAngle = maxSteerAngle;
    }

    public Double getSteerAngleRate() {
        return steerAngleRate;
    }

    public void setSteerAngleRate(Double steerAngleRate) {
        this.steerAngleRate = steerAngleRate;
    }

    public Double getMaxSteerAngleRate() {
        return maxSteerAngleRate;
    }

    public void setMaxSteerAngleRate(Double maxSteerAngleRate) {
        this.maxSteerAngleRate = maxSteerAngleRate;
    }

    public Double getSteerTorque() {
        return steerTorque;
    }

    public void setSteerTorque(Double steerTorque) {
        this.steerTorque = steerTorque;
    }

    public Double getBrake() {
        return brake;
    }

    public void setBrake(Double brake) {
        this.brake = brake;
    }

    public Double getBrakeDeadZone() {
        return brakeDeadZone;
    }

    public void setBrakeDeadZone(Double brakeDeadZone) {
        this.brakeDeadZone = brakeDeadZone;
    }

    public Double getThrottle() {
        return throttle;
    }

    public void setThrottle(Double throttle) {
        this.throttle = throttle;
    }

    public Double getThrottleDeadZone() {
        return throttleDeadZone;
    }

    public void setThrottleDeadZone(Double throttleDeadZone) {
        this.throttleDeadZone = throttleDeadZone;
    }

    public GearPosition getGear() {
        return gear;
    }

    public void setGear(GearPosition gear) {
        this.gear = gear;
    }

    public TurnState getTurn() {
        return turn;
    }

    public void setTurn(TurnState turn) {
        this.turn = turn;
    }

    public Horn getHorn() {
        return horn;
    }

    public void setHorn(Horn horn) {
        this.horn = horn;
    }

    public HeadLight getHeadLight() {
        return headLight;
    }

    public void setHeadLight(HeadLight headLight) {
        this.headLight = headLight;
    }

    public Wiper getWiper() {
        return wiper;
    }

    public void setWiper(Wiper wiper) {
        this.wiper = wiper;
    }

    public Double getFlSpeed() {
        return flSpeed;
    }

    public void setFlSpeed(Double flSpeed) {
        this.flSpeed = flSpeed;
    }

    public Double getFrSpeed() {
        return frSpeed;
    }

    public void setFrSpeed(Double frSpeed) {
        this.frSpeed = frSpeed;
    }

    public Double getRlSpeed() {
        return rlSpeed;
    }

    public void setRlSpeed(Double rlSpeed) {
        this.rlSpeed = rlSpeed;
    }

    public Double getRrSpeed() {
        return rrSpeed;
    }

    public void setRrSpeed(Double rrSpeed) {
        this.rrSpeed = rrSpeed;
    }

    public Double getFlMass() {
        return flMass;
    }

    public void setFlMass(Double flMass) {
        this.flMass = flMass;
    }

    public Double getFrMass() {
        return frMass;
    }

    public void setFrMass(Double frMass) {
        this.frMass = frMass;
    }

    public Double getRlMass() {
        return rlMass;
    }

    public void setRlMass(Double rlMass) {
        this.rlMass = rlMass;
    }

    public Double getRrMass() {
        return rrMass;
    }

    public void setRrMass(Double rrMass) {
        this.rrMass = rrMass;
    }

    public Double getFlAngle() {
        return flAngle;
    }

    public void setFlAngle(Double flAngle) {
        this.flAngle = flAngle;
    }

    public Double getFrAngle() {
        return frAngle;
    }

    public void setFrAngle(Double frAngle) {
        this.frAngle = frAngle;
    }

    public Double getEngineSpeed() {
        return engineSpeed;
    }

    public void setEngineSpeed(Double engineSpeed) {
        this.engineSpeed = engineSpeed;
    }

    public Double getEngineTorque() {
        return engineTorque;
    }

    public void setEngineTorque(Double engineTorque) {
        this.engineTorque = engineTorque;
    }

    public Double getFrontEndLateralSpeed() {
        return frontEndLateralSpeed;
    }

    public void setFrontEndLateralSpeed(Double frontEndLateralSpeed) {
        this.frontEndLateralSpeed = frontEndLateralSpeed;
    }

    public Double getRearEndLateralSpeed() {
        return rearEndLateralSpeed;
    }

    public void setRearEndLateralSpeed(Double rearEndLateralSpeed) {
        this.rearEndLateralSpeed = rearEndLateralSpeed;
    }

    public Double getFrontCorneringStiffness() {
        return frontCorneringStiffness;
    }

    public void setFrontCorneringStiffness(Double frontCorneringStiffness) {
        this.frontCorneringStiffness = frontCorneringStiffness;
    }

    public Double getRearCorneringStiffness() {
        return rearCorneringStiffness;
    }

    public void setRearCorneringStiffness(Double rearCorneringStiffness) {
        this.rearCorneringStiffness = rearCorneringStiffness;
    }

    public Double getFlLightPosX() {
        return flLightPosX;
    }

    public void setFlLightPosX(Double flLightPosX) {
        this.flLightPosX = flLightPosX;
    }

    public Double getFlLightPosY() {
        return flLightPosY;
    }

    public void setFlLightPosY(Double flLightPosY) {
        this.flLightPosY = flLightPosY;
    }

    public Double getFlLightPosZ() {
        return flLightPosZ;
    }

    public void setFlLightPosZ(Double flLightPosZ) {
        this.flLightPosZ = flLightPosZ;
    }

    public Double getRlLightPosX() {
        return rlLightPosX;
    }

    public void setRlLightPosX(Double rlLightPosX) {
        this.rlLightPosX = rlLightPosX;
    }

    public Double getRlLightPosY() {
        return rlLightPosY;
    }

    public void setRlLightPosY(Double rlLightPosY) {
        this.rlLightPosY = rlLightPosY;
    }

    public Double getRlLightPosZ() {
        return rlLightPosZ;
    }

    public void setRlLightPosZ(Double rlLightPosZ) {
        this.rlLightPosZ = rlLightPosZ;
    }

    public Double getPitchToGround() {
        return pitchToGround;
    }

    public void setPitchToGround(Double pitchToGround) {
        this.pitchToGround = pitchToGround;
    }

    /**
     * 获取通用样本协议
     */
    @Override
    public String getGeneralSampleProtocol() {
        return SUBJECT_PROTOCOL;
    }
    
    /**
     * 获取通用样本协议列表
     */
    @Override
    public String[] getGeneralSampleProtocols() {
        return PROTOCOLS;
    }
    
    /**
     * 将车辆样本转换为通用样本
     */
    public GeneralSample toGeneralSample() {
        GeneralSample sample = new GeneralSample();
        sample.setTime(this);
        sample.setProtocol(SUBJECT_PROTOCOL);
        
        int contPointCount = contour == null ? 0 : contour.length;
        int trajPointCount = trajectory == null ? 0 : trajectory.length;
        List<GeneralElement> data = new ArrayList<>(80 + (contPointCount + trajPointCount) * 2);
        
        // 分配足够大小的数组
        for (int i = 0; i < 80 + (contPointCount + trajPointCount) * 2; i++) {
            data.add(new GeneralElement());
        }
        
        data.set(0, new GeneralElement((double)contPointCount));
        data.set(1, new GeneralElement((double)trajPointCount));
        data.set(2, new GeneralElement(speed));
        if (yawRate != null) data.set(3, new GeneralElement(yawRate));
        if (curvature != null) data.set(4, new GeneralElement(curvature));
        if (accelX != null) data.set(5, new GeneralElement(accelX));
        if (accelY != null) data.set(6, new GeneralElement(accelY));
        data.set(7, new GeneralElement(vehicleWidth));
        data.set(8, new GeneralElement(vehicleLength));
        data.set(9, new GeneralElement(vehicleHeight));
        data.set(10, new GeneralElement(steerAngleRatio));
        data.set(11, new GeneralElement(wheelBase));
        data.set(12, new GeneralElement(rearTread));
        data.set(13, new GeneralElement(frontOverhang));
        if (steerAngle != null) data.set(14, new GeneralElement(steerAngle));
        if (steerAngleRate != null) data.set(15, new GeneralElement(steerAngleRate));
        if (brake != null) data.set(16, new GeneralElement(brake));
        if (throttle != null) data.set(17, new GeneralElement(throttle));
        data.set(18, new GeneralElement((double)gear.getValue()));
        data.set(19, new GeneralElement((double)turn.getValue()));
        if (flSpeed != null) data.set(20, new GeneralElement(flSpeed));
        if (frSpeed != null) data.set(21, new GeneralElement(frSpeed));
        if (rlSpeed != null) data.set(22, new GeneralElement(rlSpeed));
        if (rrSpeed != null) data.set(23, new GeneralElement(rrSpeed));
        if (engineSpeed != null) data.set(24, new GeneralElement(engineSpeed));
        if (engineTorque != null) data.set(25, new GeneralElement(engineTorque));
        data.set(26, new GeneralElement((double)horn.getValue()));
        data.set(27, new GeneralElement((double)headLight.getValue()));
        data.set(28, new GeneralElement((double)wiper.getValue()));
        data.set(29, new GeneralElement(kilometerAge));
        if (frontEndLateralSpeed != null) data.set(30, new GeneralElement(frontEndLateralSpeed));
        if (rearEndLateralSpeed != null) data.set(31, new GeneralElement(rearEndLateralSpeed));
        if (steerTorque != null) data.set(32, new GeneralElement(steerTorque));
        if (jerkX != null) data.set(33, new GeneralElement(jerkX));
        if (jerkY != null) data.set(34, new GeneralElement(jerkY));
        if (maxCurvature != null) data.set(35, new GeneralElement(maxCurvature));
        if (maxAcceleration != null) data.set(36, new GeneralElement(maxAcceleration));
        if (maxDeceleration != null) data.set(37, new GeneralElement(maxDeceleration));
        if (maxAccelY != null) data.set(38, new GeneralElement(maxAccelY));
        if (maxSteerAngle != null) data.set(39, new GeneralElement(maxSteerAngle));
        if (maxSteerAngleRate != null) data.set(40, new GeneralElement(maxSteerAngleRate));
        if (brakeDeadZone != null) data.set(41, new GeneralElement(brakeDeadZone));
        if (throttleDeadZone != null) data.set(42, new GeneralElement(throttleDeadZone));
        if (flMass != null) data.set(43, new GeneralElement(flMass));
        if (frMass != null) data.set(44, new GeneralElement(frMass));
        if (rlMass != null) data.set(45, new GeneralElement(rlMass));
        if (rrMass != null) data.set(46, new GeneralElement(rrMass));
        if (flAngle != null) data.set(47, new GeneralElement(flAngle));
        if (frAngle != null) data.set(48, new GeneralElement(frAngle));
        if (frontCorneringStiffness != null) data.set(49, new GeneralElement(frontCorneringStiffness));
        if (rearCorneringStiffness != null) data.set(50, new GeneralElement(rearCorneringStiffness));
        if (flLightPosX != null) data.set(51, new GeneralElement(flLightPosX));
        if (flLightPosY != null) data.set(52, new GeneralElement(flLightPosY));
        if (flLightPosZ != null) data.set(53, new GeneralElement(flLightPosZ));
        if (rlLightPosX != null) data.set(54, new GeneralElement(rlLightPosX));
        if (rlLightPosY != null) data.set(55, new GeneralElement(rlLightPosY));
        if (rlLightPosZ != null) data.set(56, new GeneralElement(rlLightPosZ));
        if (pitchToGround != null) data.set(57, new GeneralElement(pitchToGround));
        
        // 添加轮廓点
        for (int i = 0; i < contPointCount; i++) {
            data.set(80 + 2 * i, new GeneralElement((double)contour[i].getX()));
            data.set(80 + 2 * i + 1, new GeneralElement((double)contour[i].getY()));
        }
        
        // 添加轨迹点
        int trajectoryBase = 80 + contPointCount * 2;
        for (int i = 0; i < trajPointCount; i++) {
            data.set(trajectoryBase + 2 * i, new GeneralElement((double)trajectory[i].getX()));
            data.set(trajectoryBase + 2 * i + 1, new GeneralElement((double)trajectory[i].getY()));
        }
        
        // 移除null元素
        List<GeneralElement> result = new ArrayList<>();
        for (GeneralElement element : data) {
            if (element != null) {
                result.add(element);
            } else {
                result.add(new GeneralElement(0.0)); // 使用默认值填充null元素
            }
        }
        
        sample.setValues(result.toArray(new GeneralElement[0]));
        sample.setNumberOfSignificants(80 + contPointCount * 2);
        
        return sample;
    }

    /**
     * 从通用样本转换为车辆样本
     */
    @Override
    public boolean fromGeneralSample(GeneralSample sample) {
        setTime(sample);
        
        if (SUBJECT_PROTOCOL.equals(sample.getProtocol()) || OBJECT_PROTOCOL.equals(sample.getProtocol())) {
            return fromVehicleSampleV5(sample);
        } else if ("vehicle-sample-v4".equals(sample.getProtocol())) {
            return fromVehicleSampleV4(sample);
        } else if ("vehicle-sample-v3".equals(sample.getProtocol())) {
            return fromVehicleSampleV3(sample);
        }
        
        return false;
    }
    
    /**
     * 从VehicleSampleV5通用样本转换
     */
    private boolean fromVehicleSampleV5(GeneralSample sample) {
        List<GeneralElement> elements = Arrays.asList(sample.getValues());
        if (elements.size() < 80) return false;
        
        int contourSize = (int)elements.get(0).getValue();
        int trajectorySize = (int)elements.get(1).getValue();
        int sizeWithoutTraj = 80 + 2 * contourSize;
        int sizeWithTraj = 80 + 2 * (contourSize + trajectorySize);
        
        if (elements.size() != sizeWithoutTraj && elements.size() != sizeWithTraj) return false;
        
        speed = elements.get(2).isValid() ? elements.get(2).getValue() : 0;
        yawRate = getDoubleOrNull(elements.get(3));
        curvature = getDoubleOrNull(elements.get(4));
        accelX = getDoubleOrNull(elements.get(5));
        accelY = getDoubleOrNull(elements.get(6));
        vehicleWidth = elements.get(7).isValid() ? elements.get(7).getValue() : 1.9;
        vehicleLength = elements.get(8).isValid() ? elements.get(8).getValue() : 4.6;
        vehicleHeight = elements.get(9).isValid() ? elements.get(9).getValue() : 1.5;
        steerAngleRatio = elements.get(10).isValid() ? elements.get(10).getValue() : 15;
        wheelBase = elements.get(11).isValid() ? elements.get(11).getValue() : 2.8;
        rearTread = elements.get(12).isValid() ? elements.get(12).getValue() : 1.6;
        frontOverhang = elements.get(13).isValid() ? elements.get(13).getValue() : 0.9;
        steerAngle = getDoubleOrNull(elements.get(14));
        steerAngleRate = getDoubleOrNull(elements.get(15));
        brake = getDoubleOrNull(elements.get(16));
        throttle = getDoubleOrNull(elements.get(17));
        gear = elements.get(18).isValid() ? GearPosition.fromValue((int)elements.get(18).getValue()) : GearPosition.UNKNOWN;
        turn = elements.get(19).isValid() ? TurnState.fromValue((int)elements.get(19).getValue()) : TurnState.UNKNOWN;
        flSpeed = getDoubleOrNull(elements.get(20));
        frSpeed = getDoubleOrNull(elements.get(21));
        rlSpeed = getDoubleOrNull(elements.get(22));
        rrSpeed = getDoubleOrNull(elements.get(23));
        engineSpeed = getDoubleOrNull(elements.get(24));
        engineTorque = getDoubleOrNull(elements.get(25));
        horn = elements.get(26).isValid() ? Horn.fromValue((int)elements.get(26).getValue()) : Horn.UNKNOWN;
        headLight = elements.get(27).isValid() ? HeadLight.fromValue((int)elements.get(27).getValue()) : HeadLight.UNKNOWN;
        wiper = elements.get(28).isValid() ? Wiper.fromValue((int)elements.get(28).getValue()) : Wiper.UNKNOWN;
        kilometerAge = elements.get(29).isValid() ? elements.get(29).getValue() : 0;
        frontEndLateralSpeed = getDoubleOrNull(elements.get(30));
        rearEndLateralSpeed = getDoubleOrNull(elements.get(31));
        steerTorque = getDoubleOrNull(elements.get(32));
        jerkX = getDoubleOrNull(elements.get(33));
        jerkY = getDoubleOrNull(elements.get(34));
        maxCurvature = getDoubleOrNull(elements.get(35));
        maxAcceleration = getDoubleOrNull(elements.get(36));
        maxDeceleration = getDoubleOrNull(elements.get(37));
        maxAccelY = getDoubleOrNull(elements.get(38));
        maxSteerAngle = getDoubleOrNull(elements.get(39));
        maxSteerAngleRate = getDoubleOrNull(elements.get(40));
        brakeDeadZone = getDoubleOrNull(elements.get(41));
        throttleDeadZone = getDoubleOrNull(elements.get(42));
        flMass = getDoubleOrNull(elements.get(43));
        frMass = getDoubleOrNull(elements.get(44));
        rlMass = getDoubleOrNull(elements.get(45));
        rrMass = getDoubleOrNull(elements.get(46));
        flAngle = getDoubleOrNull(elements.get(47));
        frAngle = getDoubleOrNull(elements.get(48));
        frontCorneringStiffness = getDoubleOrNull(elements.get(49));
        rearCorneringStiffness = getDoubleOrNull(elements.get(50));
        flLightPosX = getDoubleOrNull(elements.get(51));
        flLightPosY = getDoubleOrNull(elements.get(52));
        flLightPosZ = getDoubleOrNull(elements.get(53));
        rlLightPosX = getDoubleOrNull(elements.get(54));
        rlLightPosY = getDoubleOrNull(elements.get(55));
        rlLightPosZ = getDoubleOrNull(elements.get(56));
        pitchToGround = getDoubleOrNull(elements.get(57));
        
        // 处理轮廓点
        contour = new FloatPoint[contourSize];
        for (int i = 0; i < contourSize; i++) {
            contour[i] = new FloatPoint();
            contour[i].setX((float)elements.get(80 + 2 * i).getValue());
            contour[i].setY((float)elements.get(80 + 2 * i + 1).getValue());
        }
        
        // 处理轨迹点
        if (elements.size() == sizeWithTraj) {
            trajectory = new FloatPoint[trajectorySize];
            for (int i = 0; i < trajectorySize; i++) {
                trajectory[i] = new FloatPoint();
                trajectory[i].setX((float)elements.get(sizeWithoutTraj + 2 * i).getValue());
                trajectory[i].setY((float)elements.get(sizeWithoutTraj + 2 * i + 1).getValue());
            }
        } else {
            trajectory = new FloatPoint[0];
        }
        
        return true;
    }
    
    /**
     * 从VehicleSampleV4通用样本转换
     */
    private boolean fromVehicleSampleV4(GeneralSample sample) {
        List<GeneralElement> elements = Arrays.asList(sample.getValues());
        if (elements.size() < 40) return false;
        
        int contourSize = (int)elements.get(0).getValue();
        int trajectorySize = (int)elements.get(1).getValue();
        int sizeWithoutTraj = 40 + 2 * contourSize;
        int sizeWithTraj = 40 + 2 * (contourSize + trajectorySize);
        
        if (elements.size() != sizeWithoutTraj && elements.size() != sizeWithTraj) return false;
        
        speed = elements.get(2).isValid() ? elements.get(2).getValue() : 0;
        yawRate = getDoubleOrNull(elements.get(3));
        curvature = getDoubleOrNull(elements.get(4));
        accelX = getDoubleOrNull(elements.get(5));
        accelY = getDoubleOrNull(elements.get(6));
        vehicleWidth = elements.get(7).isValid() ? elements.get(7).getValue() : 1.9;
        vehicleLength = elements.get(8).isValid() ? elements.get(8).getValue() : 4.6;
        vehicleHeight = elements.get(9).isValid() ? elements.get(9).getValue() : 1.5;
        steerAngleRatio = elements.get(10).isValid() ? elements.get(10).getValue() : 15;
        wheelBase = elements.get(11).isValid() ? elements.get(11).getValue() : 2.8;
        rearTread = elements.get(12).isValid() ? elements.get(12).getValue() : 1.6;
        frontOverhang = elements.get(13).isValid() ? elements.get(13).getValue() : 0.9;
        steerAngle = getDoubleOrNull(elements.get(14));
        steerAngleRate = getDoubleOrNull(elements.get(15));
        brake = getDoubleOrNull(elements.get(16));
        throttle = getDoubleOrNull(elements.get(17));
        gear = elements.get(18).isValid() ? GearPosition.fromValue((int)elements.get(18).getValue()) : GearPosition.UNKNOWN;
        turn = elements.get(19).isValid() ? TurnState.fromValue((int)elements.get(19).getValue()) : TurnState.UNKNOWN;
        flSpeed = getDoubleOrNull(elements.get(20));
        frSpeed = getDoubleOrNull(elements.get(21));
        rlSpeed = getDoubleOrNull(elements.get(22));
        rrSpeed = getDoubleOrNull(elements.get(23));
        engineSpeed = getDoubleOrNull(elements.get(24));
        engineTorque = getDoubleOrNull(elements.get(25));
        horn = elements.get(26).isValid() ? Horn.fromValue((int)elements.get(26).getValue()) : Horn.UNKNOWN;
        headLight = elements.get(27).isValid() ? HeadLight.fromValue((int)elements.get(27).getValue()) : HeadLight.UNKNOWN;
        wiper = elements.get(28).isValid() ? Wiper.fromValue((int)elements.get(28).getValue()) : Wiper.UNKNOWN;
        kilometerAge = elements.get(29).isValid() ? elements.get(29).getValue() : 0;
        frontEndLateralSpeed = getDoubleOrNull(elements.get(30));
        rearEndLateralSpeed = getDoubleOrNull(elements.get(31));
        steerTorque = getDoubleOrNull(elements.get(32));
        jerkX = getDoubleOrNull(elements.get(33));
        jerkY = getDoubleOrNull(elements.get(34));
        
        // 清空V5特有字段
        maxCurvature = null;
        maxAcceleration = null;
        maxDeceleration = null;
        maxAccelY = null;
        maxSteerAngle = null;
        maxSteerAngleRate = null;
        brakeDeadZone = null;
        throttleDeadZone = null;
        flMass = null;
        frMass = null;
        rlMass = null;
        rrMass = null;
        flAngle = null;
        frAngle = null;
        frontCorneringStiffness = null;
        rearCorneringStiffness = null;
        
        // 处理轮廓点
        contour = new FloatPoint[contourSize];
        for (int i = 0; i < contourSize; i++) {
            contour[i] = new FloatPoint();
            contour[i].setX((float)elements.get(40 + 2 * i).getValue());
            contour[i].setY((float)elements.get(40 + 2 * i + 1).getValue());
        }
        
        // 处理轨迹点
        if (elements.size() == sizeWithTraj) {
            trajectory = new FloatPoint[trajectorySize];
            for (int i = 0; i < trajectorySize; i++) {
                trajectory[i] = new FloatPoint();
                trajectory[i].setX((float)elements.get(sizeWithoutTraj + 2 * i).getValue());
                trajectory[i].setY((float)elements.get(sizeWithoutTraj + 2 * i + 1).getValue());
            }
        } else {
            trajectory = new FloatPoint[0];
        }
        
        return true;
    }
    
    /**
     * 从VehicleSampleV3通用样本转换
     */
    private boolean fromVehicleSampleV3(GeneralSample sample) {
        List<GeneralElement> elements = Arrays.asList(sample.getValues());
        if (elements.size() < 27) return false;
        
        int trajectorySize = (int)elements.get(0).getValue();
        int sizeWithoutTraj = 27;
        int sizeWithTraj = 27 + 2 * trajectorySize;
        
        if (elements.size() != sizeWithoutTraj && elements.size() != sizeWithTraj) return false;
        
        speed = elements.get(1).isValid() ? elements.get(1).getValue() : 0;
        yawRate = getDoubleOrNull(elements.get(2));
        curvature = getDoubleOrNull(elements.get(3));
        accelX = getDoubleOrNull(elements.get(4));
        accelY = getDoubleOrNull(elements.get(5));
        vehicleWidth = elements.get(6).isValid() ? elements.get(6).getValue() : 1.9;
        vehicleLength = elements.get(7).isValid() ? elements.get(7).getValue() : 4.6;
        vehicleHeight = 1.5;
        steerAngleRatio = elements.get(8).isValid() ? elements.get(8).getValue() : 15;
        wheelBase = elements.get(9).isValid() ? elements.get(9).getValue() : 2.8;
        rearTread = elements.get(10).isValid() ? elements.get(10).getValue() : 1.6;
        frontOverhang = elements.get(11).isValid() ? elements.get(11).getValue() : 0.9;
        steerAngle = getDoubleOrNull(elements.get(12));
        steerAngleRate = null;
        brake = getDoubleOrNull(elements.get(13));
        throttle = getDoubleOrNull(elements.get(14));
        gear = elements.get(15).isValid() ? GearPosition.fromValue((int)elements.get(15).getValue()) : GearPosition.UNKNOWN;
        turn = elements.get(16).isValid() ? TurnState.fromValue((int)elements.get(16).getValue()) : TurnState.UNKNOWN;
        flSpeed = getDoubleOrNull(elements.get(17));
        frSpeed = getDoubleOrNull(elements.get(18));
        rlSpeed = getDoubleOrNull(elements.get(19));
        rrSpeed = getDoubleOrNull(elements.get(20));
        engineSpeed = getDoubleOrNull(elements.get(21));
        engineTorque = getDoubleOrNull(elements.get(22));
        horn = elements.get(23).isValid() ? Horn.fromValue((int)elements.get(23).getValue()) : Horn.UNKNOWN;
        headLight = elements.get(24).isValid() ? HeadLight.fromValue((int)elements.get(24).getValue()) : HeadLight.UNKNOWN;
        wiper = elements.get(25).isValid() ? Wiper.fromValue((int)elements.get(25).getValue()) : Wiper.UNKNOWN;
        kilometerAge = elements.get(26).isValid() ? elements.get(26).getValue() : 0;
        
        // 清空V4和V5特有字段
        frontEndLateralSpeed = null;
        rearEndLateralSpeed = null;
        steerTorque = null;
        jerkX = null;
        jerkY = null;
        maxCurvature = null;
        maxAcceleration = null;
        maxDeceleration = null;
        maxAccelY = null;
        maxSteerAngle = null;
        maxSteerAngleRate = null;
        brakeDeadZone = null;
        throttleDeadZone = null;
        flMass = null;
        frMass = null;
        rlMass = null;
        rrMass = null;
        flAngle = null;
        frAngle = null;
        frontCorneringStiffness = null;
        rearCorneringStiffness = null;
        
        // 处理轨迹点
        if (elements.size() == sizeWithTraj) {
            trajectory = new FloatPoint[trajectorySize];
            for (int i = 0; i < trajectorySize; i++) {
                trajectory[i] = new FloatPoint();
                trajectory[i].setX((float)elements.get(27 + 2 * i).getValue());
                trajectory[i].setY((float)elements.get(27 + 2 * i + 1).getValue());
            }
        } else {
            trajectory = new FloatPoint[0];
        }
        
        contour = new FloatPoint[0];
        
        return true;
    }
    
    /**
     * 从GeneralElement获取Double或null
     */
    private Double getDoubleOrNull(GeneralElement element) {
        return element != null && element.isValid() ? element.getValue() : null;
    }
    
    /**
     * 是否支持插值
     */
    @Override
    public boolean supportInterpolation() {
        return true;
    }
    
    /**
     * 插值生成中间样本
     */
    @Override
    public Sample interpolate(SearchResult result) {
        VehicleSample s1 = (VehicleSample) result.getS1();
        VehicleSample s2 = (VehicleSample) result.getS2();
        double w1 = result.getW1();
        double w2 = result.getW2();
        
        VehicleSample output = new VehicleSample();
        
        output.speed = s1.speed * w1 + s2.speed * w2;
        output.kilometerAge = s1.kilometerAge * w1 + s2.kilometerAge * w2;
        output.yawRate = interpolateDouble(s1.yawRate, s2.yawRate, w1, w2);
        output.curvature = interpolateDouble(s1.curvature, s2.curvature, w1, w2);
        output.accelX = interpolateDouble(s1.accelX, s2.accelX, w1, w2);
        output.accelY = interpolateDouble(s1.accelY, s2.accelY, w1, w2);
        output.jerkX = interpolateDouble(s1.jerkX, s2.jerkX, w1, w2);
        output.jerkY = interpolateDouble(s1.jerkY, s2.jerkY, w1, w2);
        
        // 常量参数直接使用s1的值
        output.vehicleWidth = s1.vehicleWidth;
        output.vehicleLength = s1.vehicleLength;
        output.vehicleHeight = s1.vehicleHeight;
        output.steerAngleRatio = s1.steerAngleRatio;
        output.wheelBase = s1.wheelBase;
        output.rearTread = s1.rearTread;
        output.frontOverhang = s1.frontOverhang;
        
        output.steerAngle = interpolateDouble(s1.steerAngle, s2.steerAngle, w1, w2);
        output.steerAngleRate = interpolateDouble(s1.steerAngleRate, s2.steerAngleRate, w1, w2);
        output.steerTorque = interpolateDouble(s1.steerTorque, s2.steerTorque, w1, w2);
        output.brake = interpolateDouble(s1.brake, s2.brake, w1, w2);
        output.throttle = interpolateDouble(s1.throttle, s2.throttle, w1, w2);
        
        // 枚举类型使用权重较大的
        output.gear = w1 > w2 ? s1.gear : s2.gear;
        output.turn = w1 > w2 ? s1.turn : s2.turn;
        output.horn = w1 > w2 ? s1.horn : s2.horn;
        output.headLight = w1 > w2 ? s1.headLight : s2.headLight;
        output.wiper = w1 > w2 ? s1.wiper : s2.wiper;
        
        output.flSpeed = interpolateDouble(s1.flSpeed, s2.flSpeed, w1, w2);
        output.frSpeed = interpolateDouble(s1.frSpeed, s2.frSpeed, w1, w2);
        output.rlSpeed = interpolateDouble(s1.rlSpeed, s2.rlSpeed, w1, w2);
        output.rrSpeed = interpolateDouble(s1.rrSpeed, s2.rrSpeed, w1, w2);
        output.engineSpeed = interpolateDouble(s1.engineSpeed, s2.engineSpeed, w1, w2);
        output.engineTorque = interpolateDouble(s1.engineTorque, s2.engineTorque, w1, w2);
        output.frontEndLateralSpeed = interpolateDouble(s1.frontEndLateralSpeed, s2.frontEndLateralSpeed, w1, w2);
        output.rearEndLateralSpeed = interpolateDouble(s1.rearEndLateralSpeed, s2.rearEndLateralSpeed, w1, w2);
        
        // 常量型参数使用s1的值
        output.maxCurvature = s1.maxCurvature;
        output.maxAcceleration = s1.maxAcceleration;
        output.maxDeceleration = s1.maxDeceleration;
        output.maxAccelY = s1.maxAccelY;
        output.maxSteerAngle = s1.maxSteerAngle;
        output.maxSteerAngleRate = s1.maxSteerAngleRate;
        output.brakeDeadZone = s1.brakeDeadZone;
        output.throttleDeadZone = s1.throttleDeadZone;
        output.flMass = s1.flMass;
        output.frMass = s1.frMass;
        output.rlMass = s1.rlMass;
        output.rrMass = s1.rrMass;
        
        output.flAngle = interpolateDouble(s1.flAngle, s2.flAngle, w1, w2);
        output.frAngle = interpolateDouble(s1.frAngle, s2.frAngle, w1, w2);
        
        output.frontCorneringStiffness = s1.frontCorneringStiffness;
        output.rearCorneringStiffness = s1.rearCorneringStiffness;
        
        // 大灯位置坐标使用权重较大的值
        output.flLightPosX = w1 > w2 ? s1.flLightPosX : s2.flLightPosX;
        output.flLightPosY = w1 > w2 ? s1.flLightPosY : s2.flLightPosY;
        output.flLightPosZ = w1 > w2 ? s1.flLightPosZ : s2.flLightPosZ;
        output.rlLightPosX = w1 > w2 ? s1.rlLightPosX : s2.rlLightPosX;
        output.rlLightPosY = w1 > w2 ? s1.rlLightPosY : s2.rlLightPosY;
        output.rlLightPosZ = w1 > w2 ? s1.rlLightPosZ : s2.rlLightPosZ;
        
        // 俯仰角进行线性插值
        output.pitchToGround = interpolateDouble(s1.pitchToGround, s2.pitchToGround, w1, w2);
        
        // 轨迹点和轮廓点使用权重较大的
        output.trajectory = w1 > w2 ? s1.trajectory : s2.trajectory;
        output.contour = s1.contour;
        
        return output;
    }
    
    /**
     * 插值Double类型数据，处理null情况
     */
    private Double interpolateDouble(Double v1, Double v2, double w1, double w2) {
        if (v1 == null && v2 == null) return null;
        if (v1 == null) return v2;
        if (v2 == null) return v1;
        return v1 * w1 + v2 * w2;
    }

    // 协议相关常量
    public static final String SUBJECT_PROTOCOL = "vehicle-sample-v5";
    public static final String OBJECT_PROTOCOL = "object-vehicle-sample-v1";
    public static final String[] PROTOCOLS = {
        "vehicle-sample-v3", "vehicle-sample-v4", "vehicle-sample-v5", "object-vehicle-sample-v1"
    };
}
